#include <ros/ros.h>
#include <sdfs_tcp_pkg/SENSOR_DATA.h>
#include <string.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <arpa/inet.h>

float bytesToFloat(const char* data) {
    float value;
    std::memcpy(&value, data, sizeof(value));  // 将4字节转换为浮动数
    return value;
}

int findHeadIndex(const char* buffer, int size) {
    // 遍历缓冲区中的每个字节，查找头标志
    for (int i = 0; i < size; i++) {
        if (buffer[i] == 170) {  // 假设数据头是 170
            return i;  // 返回头部索引
        }
    }
    return -1;  // 没有找到头部
}

int main(int argc, char** argv) {
    char receivebuff[102400];
    char databuff[102400];
    ros::init(argc, argv, "sensor_publisher");
    ros::NodeHandle nh;
    ros::Publisher sensor_data_pub = nh.advertise<sdfs_tcp_pkg::SENSOR_DATA>("sensor_data", 50);
    sdfs_tcp_pkg::SENSOR_DATA sensor_data;

    int fd = socket(AF_INET, SOCK_STREAM, 0);
    if (fd == -1) {
        perror("socket");
        return -1;
    }

    struct sockaddr_in localport;
    localport.sin_family = AF_INET;
    localport.sin_port = htons(4008);
    inet_pton(AF_INET, "192.168.0.108", &localport.sin_addr.s_addr);
    int ret = connect(fd, (struct sockaddr*)&localport, sizeof(localport));
    if (ret == -1) {
        perror("connect");
        return -1;
    }

    std::string set_rate = "AT+SMPF=200\r\n";
    std::string get_data_stream = "AT+GSD\r\n";

    // 发送初始化命令
    send(fd, set_rate.c_str(), set_rate.size(), 0);
    int receive_rate = recv(fd, receivebuff, sizeof(receivebuff), 0);
    if (receive_rate > 0) {
        ROS_INFO_STREAM("receive_rate: " << receivebuff);
    }
    std::memset(databuff, 0, sizeof(databuff));
    
    send(fd, get_data_stream.c_str(), get_data_stream.size(), 0);

    int number = 0;
    ros::Rate loop_rate(1);  // 设置1 Hz的频率

    while (ros::ok()) {
        int len = recv(fd, databuff, sizeof(databuff), 0);
        if (len <= 0) {
            ROS_WARN("Failed to receive data or connection closed.");
            break;
        }

        // 查找头部位置
        int headIndex = findHeadIndex(databuff, len);
        if (headIndex == -1) {
            ROS_WARN("Header not found in the received data.");
            continue;  // 如果没找到头部，继续下一轮接收
        }

        ROS_INFO_STREAM("headIndex: " << headIndex);

        // 解析数据
        float fx = bytesToFloat(databuff + headIndex + 6);  // 假设数据在头部后6个字节开始
        ROS_INFO_STREAM("fx: " << fx);

        // 填充并发布数据
        sensor_data.Fx = fx;  // 填充sensor_data消息
        sensor_data_pub.publish(sensor_data);  // 发布数据

        std::memset(databuff, 0, sizeof(databuff));  // 清空缓冲区
        loop_rate.sleep();  // 控制循环频率
    }

    close(fd);  // 关闭TCP连接
    return 0;
}
